void setup()  { 
  setupUltrasoon();
  Serial.begin(9600);
  Serial.println("bot ready");
}

void loop()  {
  sendUltrasoon(5);/// 5 sec aan 2 sec uit
  mission();
  // run bot
  delay(2000);
}

void mission(){ 
  boolean flagCaptured = false;
  boolean returnedToBase = false;
  boolean objectiveCompleted = false;
  
  // stip zoeken
  while(!objectiveCompleted){
     while(!flagCaptured){
     int stip =0;
     if(int stip = 0){
     driveMotors();
     analogRead();//lees rgb
     }
     if(int rgb = 1){//white
     stip = 1; }}
     if(int stip =1){
      flagCaptured = true;
      rgb = 0;
      secureFlag();}
    }
  
    // begin positie zoeken
    while(!returnedToBase){
      //lees rgb
      if(rgb = 1){
      returnedToBase = true;
      }
    }
    
    if(flagCaptured && returnedToBase){
      objectiveCompleted = true;
      // game over
      doVictoryDance();
      terminateDrone();
      
    }
  }
}

void terminateDrone(){
  while(1){
    Serial.print("zZ");//(shutdown)
    return;
  }
}

void secureFlag(){
    Serial.print("\nmusic and lightshow");
}

void doVictoryDance(){
  int led = 1;
  Serial.print("\nYEAH!!!");
}

void driveMotors(int left, int right){
  if(drive = 1)(
  Left = 1;
  Right = 1;
  )
  
  Serial.print("\ndrive");
}
void Rechtdoor(int leftS = 0, int rightS = 0){
  analogRead();//lees links rechts sensors 
  if(leftS => rightS){
    left = 0;
    delay (200);
  }
   if(leftS <= rightS){
    right = 0;
    delay (200);
  }
      
    
    
void setupUltrasoon(){
  TCCR1A = _BV(COM1A1) | _BV(WGM11); //fast PWM-mode
  TCCR1B = _BV(WGM12) | _BV(WGM13) | _BV(CS10);//fast PWM-mode en N=1
  ICR1 = 399; // PWMfreq= Fclk/(N.(1+TOP) ICR1=TOP 16.000.000/(1.(1+399)) = 40kHz
  pinMode(9, OUTPUT); // pin 9 als output
}

void sendUltrasoon(int len){
  Serial.print("\nultra");
  for(int x = 0; x < len; x++){
    for(int fadeValue = 0 ; fadeValue <= 399; fadeValue +=1) { 
    analogWrite(9, fadeValue);         
    delay(1);                            
    }
    
    for(int fadeValue = 399 ; fadeValue >= 0; fadeValue -=1) { 
      analogWrite(9, fadeValue);         
      delay(1);                            
    }
    Serial.print(".");
  }
}
